#include "Observation.h"
#include "ParticleFilter.h"
#include "math.h"

Observation::Observation() :
    distToA(0.0),
    distToB(0.0)
{
}

Observation::Observation(double distanceToA, double distanceToB) :
    distToA(distanceToA),
    distToB(distanceToB)
{
}

Observation::Observation(QString distanceToA, QString distanceToB) :
    distToA(distanceToA.toDouble()),
    distToB(distanceToB.toDouble())
{

}

Observation::Observation(const Observation &observation) :
    distToA(observation.distToA),
    distToB(observation.distToB)
{

}

double Observation::getLikelihood(double sampleX, double sampleY)
{
    double sampleDistToA = sqrt(pow(-100 - sampleX, 2.0) + pow(100 - sampleY, 2.0));

    double sampleDistToB = sqrt(pow(150 - sampleX, 2.0) + pow(90 - sampleY, 2.0));

    // remember: sigma = 1
    double likelihoodA = exp(-pow(distToA - sampleDistToA, 2.0) / 2.0) / sqrt(2 * M_PI * 1);
    double likelihoodB = exp(-pow(distToB - sampleDistToB, 2.0) / 2.0) / sqrt(2 * M_PI * 1);

    return likelihoodA * likelihoodB;
}
